import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from go2_tutorial_inter.srv import Cruising
from geometry_msgs.msg import Point

from go2_tutorial_py.sport_model import *
from rcl_interfaces.srv import SetParameters
from rcl_interfaces.msg import Parameter
from rcl_interfaces.msg import ParameterValue
from rcl_interfaces.msg import ParameterType
#这是巡航的服务端节点实现



# 自定义节点类
class CruServer(Node):
    def __init__(self):
        super().__init__('cru_server_node_node')

        # 声明参数
        self.declare_parameter('vx', 0.0)
        self.declare_parameter('vy', 0.0)
        self.declare_parameter('vyaw', 0.5)

        # 创建参数服务客户端，连接到速度发布节点
        self.param_client = self.create_client(SetParameters, '/go2_ctrl_node_py/set_parameters')

        # 等待服务连接
        while not self.param_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error('Interrupted while waiting for the service. Exiting.')
                return
            self.get_logger().info('服务未连接')

        self.get_logger().info('已经连接成功速度发送节点的参数服务，可以设置线速度和角速度了')

        # 创建订阅方，订阅机器人的里程计以获取机器人坐标
        self.sub_odom_ = self.create_subscription(Odometry, 'odom', self.on_timer, 10)

        # 创建服务端，处理客户端请求
        self.service = self.create_service(Cruising, 'cruising', self.cb)

        # 初始化坐标
        self.current_point = Point()

    def cb(self, request, response):
        flag = request.flag
        id = ROBOT_SPORT_API_IDS["BALANCESTAND"]  

        # 判断提交的数据
        if flag != 0:  # 开始巡航
            self.get_logger().info('开始巡航......')
            id = ROBOT_SPORT_API_IDS["MOVE"]
        else:  # 结束巡航
            self.get_logger().info('终止巡航......')

        # 设置参
        req = SetParameters.Request()

        # 运动模式
        p1 = Parameter()

        p1.name = "sport_api_id"

        v1 = ParameterValue()
        v1.type = ParameterType.PARAMETER_INTEGER
        v1.integer_value = id

        p1.value = v1

        # x线速度
        p2 = Parameter()

        p2.name = "vx"

        v2 = ParameterValue()
        v2.type = ParameterType.PARAMETER_DOUBLE
        v2.double_value = self.get_parameter("vx").value

        p2.value = v2

        # y线速度
        p3 = Parameter()

        p3.name = "vy"

        v3 = ParameterValue()
        v3.type = ParameterType.PARAMETER_DOUBLE
        v3.double_value = self.get_parameter("vy").value

        p3.value = v3

        # 角速度
        p4 = Parameter()

        p4.name = "vyaw"

        v4 = ParameterValue()
        v4.type = ParameterType.PARAMETER_DOUBLE
        v4.double_value = self.get_parameter("vyaw").value

        p4.value = v4

        req.parameters = [
            p1,
            p2,
            p3,
            p4
        ]

        self.param_client.call_async(req)

        # 返回当前坐标
        response.point = self.current_point
        return response

    def on_timer(self, odom: Odometry):
        self.current_point = odom.pose.pose.position

def main(args=None):
    # 初始化ROS2客户端
    rclpy.init(args=args)

    # 创建节点并运行
    cru_server = CruServer()
    rclpy.spin(cru_server)

    # 资源释放
    cru_server.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()